#ifndef AEROROBOTCONTROL_H
#define AEROROBOTCONTROL_H

/******************************************************************************
 * @file        AeroRobotControl.h
 * @brief       为用户提供二次开发的对象库。
 * @details     包含一个类AeroRobotControl。定义一个对象以代表要进行控制的机械臂。
 * @author      ARC<1131970238@qq.com>
 * @date        2022-06-27
 * @version     v1.9
 * @copyright   Copyright By ARC, All Rights Reserved.
 *
 ******************************************************************************
 *
 * <table>
 * @par 修改日志 Data: 2022-03-15 Version: v1.0
 * @par 描述：添加新的注释信息；添加远程编程的函数接口。
 * </table>
 *
 *
 * <table>
 * @par 修改日志 Data: 2022-04-16 Version: v1.1
 * @par 描述：添加获取双编码差值的接口，还未实现。
 * </table>
 *
 * <table>
 * @par 修改日志 Data: 2022-04-17 Version: v1.2
 * @par 描述：实现双编码差值的接口；添加修改广播频率的接口。
 * </table>
 *
 *
 * <table>
 * @par 修改日志 Data: 2022-04-17 Version: v1.3
 * @par 描述： 支持快速编程。
 * </table>
 *
 * <table>
 * @par 修改日志 Data: 2022-04-20 Version: v1.4
 * @par 描述： 添加基于TCP的增量点位运动。
 * </table>
 *
 * <table>
 * @par 修改日志 Data: 2022-04-21 Version: v1.5
 * @par 描述： 添加以注册数据指针的方式获取数据。
 * </table>
 *
 * <table>
 * @par 修改日志 Data: 2022-04-22 Version: v1.6
 * @par 描述：获取机器人状态。
 * </table>
 *
 * <table>
 * @par 修改日志 Data: 2022-04-26 Version: v1.7
 * @par 描述：实现机器人inching获取。
 * </table>
 *
 *
 * <table>
 * @par 修改日志 Data: 2022-05-28 Version: v1.8
 * @par 描述：控制客户端是否打印错误。
 * </table>
 *
 *
 * <table>
 * @par 修改日志 Data: 2022-06-27 Version: v1.9
 * @par 描述：修改了双编码获取数值的类型，需要结合0627版本的控制器使用。
 * </table>
 *****************************************************************************/



#include <memory>
#include "ClientOne.h"
#include "ClientThree.h"

namespace AeroRobot {


/**
 * @brief The AeroRobotControl class 机械臂运动控制类。
 */
class AeroRobotControl
{
public:

    //======================================================================== 初始化函数 =====================================================================

    /**
     * @brief AeroRobotControl 构造函数初始化时需要输入机械臂的IP。
     * @param IP
     */
    AeroRobotControl(const char * IP);


    /**
     * @brief connectToRobot 连接机械臂。
     * @return 连接成功如果返回true,否则返回false。
     */
    bool connectToRobot();

    //=======================================================================================================================================================



    //====================================================================== 机械臂通用控制 ====================================================================

    /**
     * @brief setEmergencyOn 打开机器人的急停。机器人会停止当前的一切运动并不在响应后续的运动。
     */
    void setEmergencyOn();


    /**
     * @brief setEmergencyOff 关闭机器人急停。机器人可以继续响应运动命令。
     */
    void setEmergencyOff();


    /**
     * @brief setServoOn 为机器人上伺服。
     */
    void setServoOn();


    /**
     * @brief setServoOff 为机器人下伺服。
     */
    void setServoOff();


    /**
     * @brief setHomePos 设置当前位置为Home位置.
     * @return 成功如果返回true,否则返回false。
     */
    bool setHomePos();


    /**
     * @brief moveToHomePos 运动到Home位置.
     * @return 成功如果返回true,否则返回false。
     */
    bool moveToHomePos(double s);


    /**
     * @brief moveToZeroPos 运动到Zero位置.
     * @return 成功如果返回true,否则返回false。
     */
    bool moveToZeroPos(double s);



    /**
     * @brief changeBroadcastCycTime 改变广播周期。
     * @param time 单位是ms,支持修改最快8ms。
     */
    void changeBroadcastCycTime(int time);



    //=======================================================================================================================================================


    //====================================================================== 机械臂状态获取 ====================================================================
    /**
     * @brief getCurrentPoseJointSpace 获取机器人在关节空间的当前位置。
     * @param pos_ptr 需要用户提前定义一个double 6 的数组，并将数组的地址作为输入参数。数据会被更新到用户数组中。单位是rad。
     */
    void getCurrentPoseJointSpace(double * pos_ptr);



    /**
     * @brief getCurrentPoseCartSpace 获取机器人在笛卡尔空间的当前位置和姿态。
     * @param pos_ptr 需要用户提前定义一个double 6 的数组，并将数组的地址作为输入参数。数据会被更新到用户数组中。
     *                前三个数值分别是x,y,z，单位是m；后三个数值是rz,ry,rx的欧拉角（也是求取顺序），单位是rad。
     */
    void getCurrentPoseCartSpace(double * pos_ptr);



    /**
     * @brief getCurrentSpeedJointSpace  获取机器人在关节空间的当前速度。
     * @param speed_ptr 需要用户提前定义一个double 6 的数组，并将数组的地址作为输入参数。数据会被更新到用户数组中。单位是rad/s。
     */
    void getCurrentSpeedJointSpace(double * speed_ptr);

    /**
     * @brief getCurrentAccelJointSpace
     * @param accel_ptr
     */
    void getCurrentAccelJointSpace(double * accel_ptr);


    /**
     * @brief getCurrentAmpereJointSpace  获取机器人在关节空间的当前电流。
     * @param Ampere_ptr 需要用户提前定义一个double 6 的数组，并将数组的地址作为输入参数。数据会被更新到用户数组中。单位是A。
     */
    void getCurrentAmpereJointSpace(double * Ampere_ptr);


    /**
     * @brief getDoubleEncoderDiff 获取机器人双编码器的插值。目前只有泰科RDM驱动器可以获取有效数据。不受广播频率的影响。
     * @param diff_ptr 需要用户提前定义一个double 6 的数组，并将数组的地址作为输入参数。数据会被更新到用户数组中。单位是rad。
     * @return 如果没有超过请求时间并且通讯正常返回true。
     * @todo 还未测试。
     */
    bool getDoubleEncoderDiff(double * diff_ptr);


    /**
     * @brief isHome 查询是否在Home位置。
     * @return 通信正常且在Home位置返回true。
     */
    bool isHome();


    /**
     * @brief getRobotStatusPtrRegister 用户注册RobotStatus类型的数据结构，并将数据指针传入，库函数会自动更新数据。
     * @param userData_ptr 用户传入的数据指针。
     * @attention ！！！用户需要确保在程序运行过程中，指针指向的数据内存始终是合法的、可访问的。！！！
     * @attention ！！！获取数据的单位与直接通过"get***"函数获取数据的单位可能会不一致。请用户仔细查看"RobotStatus"的注释。！！！
     * @return 注册成功返回true。
     */
    bool getRobotStatusPtrRegister(RobotStatus * userData_ptr);


    /**
     * @brief getRobotState 获取机器人的状态。
     * @param robotState 传入被更新的数据引用。
     */
    void getRobotState(RobotState & robotState);


    //========================================================================================================================================================


    //===================================================================== 机械臂JOG示教控制 ==================================================================

    //Move joint continuously.
    //Calling this function will move robot joint(int axis=1/2/.../6 represents joint 1/2.../6) forward(bool forwardMove=true) or backwarf(bool forwardMove=false).
    void MoveContinuousJoint(int axis, bool forwardMove);                           //axis: [1, 6]. forwardMove: true,false -> forward, backward.
    //Move robot end in cartesian space continuously.
    //Calling this function will move robot end along axis(int dir=1/2/3 represents x/y/z) forward(bool forwardMove=true) or backwarf(bool forwardMove=false) in two coordinate systems(int refCoordinate=0/1 represents base/TCP).
    void MoveContinuousCartesianPose(int dir, bool forwardMove, int refCoordinate); //dir: [1, 3] -> x,y,z. forwardMove: true,false -> forward, backward. refCoordinate: 0, 1 -> base, TCP
    //Rotate robot end in cartesian space continuously.
    //Calling this function will rotate robot end along axis(int dir=1/2/3 represents x/y/z) forward(bool forwardMove=true) or backwarf(bool forwardMove=false) in two coordinate systems(int refCoordinate=0/1 represents base/TCP).
    void MoveContinuousCartesianRota(int dir, bool forwardMove, int refCoordinate); //dir: [1, 3] -> rx,ry,rz. forwardMove: true,false -> forward, backward. refCoordinate: 0, 1 -> base, TCP
    //Set Jog control speed up.
    //Calling this function will set robot Jog moving faster.
    void JogSpeedUp();                                                              //Jog speed up.
    //Set Jog control speed up.
    //Calling this function will set robot Jog moving slower.
    void JogSpeedDown();                                                            //Jog speed down.
    //Set Jog control stop.
    //Calling this function will set Jog control stop.
    void JogMoveStop();                                                             //Jog move stop.

    /**
     * @brief MoveInchingCartesianPos 笛卡尔空间位置寸动。
     * @param refCoor 寸动的参考坐标系。0：基坐标系。1：TCP。
     * @param dir 移动方向：0：X方向。1：Y方向。2:Z方向。
     * @param fMove 移动正反：true：正向移动。false：反向移动。
     */
    void MoveInchingCartesianPos(int refCoor, int dir, bool fMove);


    /**
     * @brief MoveInchingCartesianRot 笛卡尔空间旋转寸动。
     * @param refCoor 寸动的参考坐标系。0：基坐标系。1：TCP。
     * @param dir 移动方向：0：RX方向。1：RY方向。2:RZ方向。
     * @param fMove 移动正反：true：正向移动。false：反向移动。
     */
    void MoveInchingCartesianRot(int refCoor, int dir, bool fMove);


    /**
     * @brief getCurrentInchingPosLevel 获取当前位置寸动的数值。
     * @return  返回当前位置寸动的数值。单位时m。
     */
    double getCurrentInchingPosLevel();


    /**
     * @brief getCurrentInchingRot 获取当前旋转寸动的数值。
     * @return  返回当前旋转寸动的数值。单位是rad。
     */
    double getCurrentInchingRotLevel();

    //========================================================================================================================================================

    //======================================================================= 机械臂点位运动 ====================================================================
    /**
     * @brief set_csp 设置机械臂处于CSP运动模式。
     * @return 通讯成功并且设置成功返回true。
     */
    bool set_csp();


    /**
     * @brief isMoveFinished 查询机械臂点位运动是否运动停止。这个函数只在CSP模式下有效。
     * @return 通许正确并且运动到位置会返回true。没有运动到位置会返回false。
     */
    bool isMoveFinished();


    /**
     * @brief set_joint_move_speed 设置机器人点位运动在关节空间的速度。这个函数只会影响CSP模式下的运动。
     * @param s 单位是rad/s。
     * @return 通讯正常并且设置成功会返回true。
     */
    bool set_joint_move_speed(double s);


    /**
     * @brief set_Task_move_speed 设置机器人点位运动在笛卡尔空间的移动/旋转速度。这个函数只会影响CSP模式下的运动。
     * @param s 是一个指向double2数组的指针，分别是移动速度和转动速度，单位分别是m/s和rad/s。
     * @return 通讯正常并且设置成功会返回true。
     */
    bool set_Task_move_speed(const double * s);


    /**
     * @brief moveJointTo 关节空间机器人点位运动。这个函数只在CSP模式下有效。
     * @param tQ 是一个指向double6数组的指针，表示期望机器人到达的绝对位置。单位是rad。
     * @return 通讯正常并且运动状态正常会返回true。
     */
    bool moveJointTo(double * tQ);


    /**
     * @brief moveJointToUntilFinish 关节空间机器人点位运动。这个函数只在CSP模式下有效。此函数是阻塞函数，直到运动完或者截止时间到才会返回。
     * @param tQ 是一个指向double6数组的指针，表示期望机器人到达的绝对位置。单位是rad。
     * @param deadline 截至时间。如果在截至时间内没有运动完会主动发送停止指令并返回。单位是s。
     * @return 通讯正确、规划正确并且在截至时间内运动完会返回true。
     */
    bool moveJointToUntilFinish(double * tQ, double deadline = 60);


    /**
     * @brief moveTaskTo 笛卡尔空间相对基坐标系的绝对直线运动。这个函数只在CSP模式下有效。
     * @param tC 指向一个double[6]数组的指针，表示期望机械臂运动到的绝对位姿。
     *           前三个数值分别是x,y,z，单位是m；后三个数值是rz,ry,rx的欧拉角（也是求取顺序），单位是rad。
     * @return 通讯正确并且规划正确会立即返回true。
     */
    bool moveTaskTo(double * tC);


    /**
     * @brief moveTaskBy 笛卡尔空间相对基坐标系的增量直线运动。这个函数只在CSP模式下有效。
     * @param tC 指向一个double[6]数组的指针，表示期望机械臂运动到的增量位姿。
     *           前三个数值分别是x,y,z，单位是m；后三个数值是rz,ry,rx的欧拉角（也是求取顺序），单位是rad。
     * @return 通讯正确并且规划正确会立即返回true。
     */
    bool moveTaskBy(double * tC);


    /**
     * @brief moveTaskBy 笛卡尔空间相对TCP坐标系的增量直线运动。这个函数只在CSP模式下有效。
     * @param tC 指向一个double[6]数组的指针，表示期望机械臂运动到的增量位姿。
     *           前三个数值分别是x,y,z，单位是m；后三个数值是rz,ry,rx的欧拉角（也是求取顺序），单位是rad。
     * @return 通讯正确并且规划正确会立即返回true。
     */
    bool moveTaskByTCP(double * tC);


    /**
     * @brief moveTaskToWithSpeed 笛卡尔空间以给定速度相对基坐标系的绝对直线运动。这个函数只在CSP模式下有效。
     * @param tC 指向一个double[8]数组的指针，表示期望机械臂运动到的绝对位姿和对应的移动/转动速度。因为指定了运动速度，所以不受函数“set_Task_move_speed”所指定的速度的影响。
     *           前三个数值分别是x,y,z，单位是m；中间三个数值是rz,ry,rx的欧拉角（也是求取顺序），单位是rad。后两个数值是移动速度和转动速度，单位分别是m/s和rad/s。
     * @return 通讯正确并且规划正确会立即返回true。
     */
    bool moveTaskToWithSpeed(double * tC);


    /**
     * @brief moveTaskTo 笛卡尔空间相对基坐标系的绝对直线运动。这个函数只在CSP模式下有效。此函数是阻塞函数，直到运动完或者截止时间到才会返回。
     * @param tC 指向一个double[6]数组的指针，表示期望机械臂运动到的绝对位姿。
     *           前三个数值分别是x,y,z，单位是m；后三个数值是rz,ry,rx的欧拉角（也是求取顺序），单位是rad。
     * @param deadline 截至时间。如果在截至时间内没有运动完会主动发送停止指令并返回。单位是s。
     * @return 通讯正确、规划正确并且在截至时间内运动完会返回true。
     */
    bool moveTaskToUntilFinish(double * tC, double deadline = 60);


    /**
     * @brief pMoveStop 立即停止一切点位运动。
     * @return 通讯正确并且停止成功会返回true。
     */
    bool pMoveStop();

    //========================================================================================================================================================


    //======================================================================= 机械臂连续运动 ====================================================================
    /**
     * @brief moveJointToContinuesWithSpeed 关节空间连续位置运动。需要给定每次运动的速度。
     * @param tJ代表指向一个double[7]数组的指针。
     *        前6个参数代表期望关节角度。单位是rad。后1个参数代表转动的速度。单位是rad/s。
     * @return 成功返回true，失败返回false。
     */
    bool moveJointToContinuesWithSpeed(double * tJ);

    /**
     * @brief moveJointToContinuesWithDurat 关节空间连续位置运动。需要给定每次运动的时间。
     * @param tJ代表指向一个double[7]数组的指针。
     *        前6个参数代表期望关节角度。单位是rad。后1个参数代表转动的时间。单位是s。
     * @return 成功返回true，失败返回false。
     */
    bool moveJointToContinuesWithDurat(double * tJ);

    /**
     * @brief moveTaskToContinues 笛卡尔空间基于基坐标系的连续位置运动。
     * @param tC代表指向一个double[8]数组的指针。
     *         前三个数值分别是x,y,z，单位是m；中间三个数值是rz,ry,rx的欧拉角（也是求取顺序），单位是rad。后两个数值是移动速度和转动速度，单位分别是m/s和rad/s。
     * @return 成功返回true，失败返回false。
     */
    bool moveTaskToContinues(double * tC);

    //=======================================================================================================================================================

    //================================================================= 机械臂复杂WayPoints运动 ===============================================================

    /**
     * @brief complexWayPointsPushBack 将复杂点加入控制器。每次调用都会在末尾加入一个新的wayoint。
     * @param tW代表指向一个double[6]数组的指针。代表要运动经过的关节空间的路径点。单位是rad。
     * @return 加入成功返回true，否则返回false。
     * @todo 已实现，还未测试。
     */
    bool complexWayPointsPushBack(double * tW);

    /**
     * @brief complexWayPointsClear 清除所有已经加入的wayPoints.
     * @return 清除成功返回true，否则返回false。
     * @todo  已实现，还未测试。
     */
    bool complexWayPointsClear();

    /**
     * @brief complexWayPointsRun 运行已经加入的所有wayPoints。
     * @return 运行成功返回true，否则返回false。
     * @todo  已实现，还未测试。
     */
    bool complexWayPointsRun();

    //=======================================================================================================================================================

    //===================================================================== 机械臂编程运动 ====================================================================

    /**
     * @brief openProg 打开程序文件。
     * @param fileName 程序文件名。
     * @param line 打开的程序的行数。
     * @param error 1：没有这个文件。2：文件内容读取失败。3:通讯错误。 0：无错误。
     * @return 打开正确返回true,否则返回false。
     * @todo 还未测试。
     */
    bool openProg(const std::string fileName, int &line, int & error);


    /**
     * @brief runProg 开始运行程序。
     * @param error 1：没有加载程序。2：机械臂处于非运行状态不可运行。3：程序点位无法到达。4：通讯错误。0：无错误。
     * @return 运行成功返回true,否则返回false。
     * @todo 还未测试。
     */
    bool runProg(int & error);

    /**
     * @brief stopProg 立即停止程序运行。
     * @param error 1：通讯错误。0：无错误。
     * @return 停止成功返回true,否则返回false。
     * @todo 还未测试。
     */
    bool stopProg(int & error);


    /**
     * @brief saveProg 保存程序。
     * @param error 1：没有加载程序。2：通讯错误。0：无错误。
     * @return 保存成功返回true,否则返回false。
     * @todo 还未测试。
     */
    bool saveProg(int & error);


    /**
     * @brief saveExitProg 保存并退出程序。
     * @param error 1：没有加载程序。2：通讯错误。0：无错误。
     * @return 保存并退出成功返回true,否则返回false。
     * @todo 还未测试。
     */
    bool saveExitProg(int & error);


    /**
     * @brief exitProg 不保存直接退出程序。
     * @param error 1：没有加载程序。2：通讯错误。0：无错误。
     * @return 退出成功返回true,否则返回false。
     * @todo 还未测试。
     */
    bool exitProg(int & error);


    /**
     * @brief pushBackPoint 将机械臂当前位置加入程序最后。
     * @param w 传入的wayPoint数据。
     * @param error 1：没有打开的程序文件。2：通讯错误。0：无错误。
     * @return 加入成功返回true,否则返回false。
     * @todo 还未测试。
     */
    bool pushBackPoint(const ProgramWayPoint & w, int &error);


    /**
     * @brief insertPoint 将机械臂当前位置插入程序。
     * @param w 传入的wayPoint数据。
     * @param error 1：没有打开的程序文件。2：插入位置大于程序记录点的数量。3：通讯错误。0：无错误。
     * @return 加入成功返回true,否则返回false。
     * @todo 还未测试。
     */
    bool insertPoint(const ProgramWayPoint & w, int &error);


    /**
     * @brief erasePoint 删除制定位置的机器人编程点。
     * @param index 删除的位置。
     * @param error 1：没有打开的程序文件。2：删除位置大于程序记录点的数量。3：通讯错误。0：无错误。
     * @return 删除成功返回true,否则返回false。
     * @todo 还未测试。
     */
    bool erasePoint(int index, int &error);


    /**
     * @brief replacePoint 将机械臂当前位置替换制定位置的机器人编程点。
     * @param w 传入的wayPoint数据。
     * @param error 1：没有打开的程序文件。2：替换位置大于程序记录点的数量。3：通讯错误。0：无错误。
     * @return 替换成功返回true,否则返回false。
     * @todo 还未测试。
     */
    bool replacePoint(const ProgramWayPoint & w, int & error);


    /**
     * @brief clearAllPoints 删除所有的已经记录的编程点。
     * @param error 1：没有打开的程序文件。2：通讯错误。0：无错误。
     * @return 删除成功返回true,否则返回false。
     * @todo 还未测试。
     */
    bool clearAllPoints(int & error);



    //=======================================================================================================================================================



    //=================================================================== 机械臂连续速度运动 ===================================================================
    /**
     * @brief set_csv 设置机器人工作在CSV模式。
     * @return 通讯成功并且设置成功返回true。
     */
    bool set_csv();


    /**
     * @brief moveJointSpeedTo 机器人关节空间连续速度运动。本函数只工作在CSV模式下。
     * @param ts 指向double6数组的指针，表示每次的期望速度。单位是rad/s。
     * @return 通讯成功并且设置成功返回true。
     */
    bool moveJointSpeedTo(const double * ts);

    //=======================================================================================================================================================


    /**
     * @brief isPrintingError 控制是否打印错误信息。
     * @param p true: 打印错误信息；false: 不打印错误信息。
     */
    void isPrintingError(bool p);

private:


    std::shared_ptr<ClientOne> clientTopic_ptr;
    std::shared_ptr<ClientThree> clientService_ptr;
};

}

#endif // AEROROBOTCONTROL_H
